#!/usr/bin/python3
# -*- coding: UTF-8 -*-
from logging import root
import numpy as np
# import cv2 as cv
import json
import matplotlib.pyplot as plt
import matplotlib.patches as patches
from typing import List
from shapely.geometry import Polygon as Rectangle
import pandas as pd
import sys
sys.path.append(sys.path[0]+"/../")

output_json = "output.json"
input_json = "input.json"


class Obstacle:
    def __init__(self, center_x, center_y, length, width, heading, counter_time=0, classification=0, score=0, height=0,
                 obj_longitude=0, obj_latitude=0):
        self.counter_time = counter_time
        self.classification = classification
        self.score = score
        self.height = height
        self.obj_longitude = obj_longitude
        self.obj_latitude = obj_latitude
        self.center_x = center_x
        self.center_y = center_y
        self.length = length
        self.width = width
        self.heading = heading


class Matplotlib_vis:
    def __init__(self, width, height):
        self.img_width = width
        self.img_height = height
        self.vanus = np.zeros((self.img_height, self.img_width, 3), np.uint8)
        self.vanus[:] = 255

    def display(self):
        plt.show()

    def calculate_rectangle(self, obs: Obstacle) -> Rectangle:
        """根据矩形长宽生成四个顶点
        默认矩形长平行与正北方向
        TODO Python 会修改原list中的obs，不用返回值

        Args:
        obs (Obstacle): 障碍物信息
        Rectangle: 四个顶点组成的矩形，从左上角按顺时针方向生成
        """
        from utils.tools import rotatation_matrix, transform_martix
        from utils.latlontomap import do_transform
        rect = Rectangle([
            [-obs.width/2, obs.length/2],
            [obs.width/2, obs.length/2],
            [obs.width/2, -obs.length/2],
            [-obs.width/2, -obs.length/2]])
        new_rect = []
        rect_rotat = rotatation_matrix(obs.heading)
        rect_trans = transform_martix(obs.center_x, obs.center_y)
        for i in range(4):
            rect_point = np.array(rect.exterior.coords[i])
            rect_point = do_transform(p=rect_point, r=rect_rotat, t=rect_trans)
            new_rect.append(rect_point)
        rectangle = Rectangle(new_rect)

        return rectangle

    def padding_vanus(self, data: List[Obstacle]):
        plt.imshow(self.vanus)
        ax = plt.gca()
        ax.yaxis.set_major_locator(plt.MultipleLocator(4.43))
        ax.xaxis.set_major_locator(plt.MultipleLocator(1.59))
        # ax.grid(which='major', axis='both', color='orangered', linewidth=0.75)
        plt.xticks(rotation=270)
        for obs in data:
            # rect = patches.Rectangle((data[i].center_x,data[i].center_y),\
            #         data[i].width, data[i].length,data[i].heading,fill = False)
            # rect
            rect = self.calculate_rectangle(obs)
            points = rect.exterior
            points = np.array(points)
            rect = patches.Polygon(points, fill=False)
            ax.add_patch(rect)
        self.display()


def parseObstacle(root):
    obs = []
    for obj in root['obstacles']:
        o = Obstacle(center_x=obj['center_x'], center_y=obj['center_y'],
                     width=obj['length'], length=obj['width'], heading=obj['heading'])
        obs.append(o)
    return obs


if __name__ == '__main__':
    test = Matplotlib_vis(105, 105)
    data = None
    # with open(output_json, mode='r') as json_file:
    with open(input_json, mode='r') as json_file:
        root = json.load(json_file)
        data = parseObstacle(root)
    data = np.array(data)
    list_test = []
    # for i in range(data.shape[0]):
    #     obj = Obstacle(data[i][0]+46, data[i][1]+41,
    #                    data[i][2], data[i][3], data[i][4])
    #     # obj = Obstacle(50,100,10,20,0)
    #     # pretty_print(obj)
    #     list_test.append(obj)

    test.padding_vanus(data)
